#include <ServoPWMF.h>

ServoPWMF s;
int val;

void setup()
{
    //setup timer prescaler
    s.setupTimer();
    Serial.begin(9600);
}

void loop()
{
      
  val=analogRead(0);
 // float angle = val/10230*180;
 // float angle = 180;
  
  if( val == 0)
    grab(s);
  else if( val > 0 && val < 200 )
    point(s);
  else if( val < 400 )
    count2(s);
  else if( val < 600 )
    count3(s);
  else if( val < 800 )
    count4(s);
  else if( val< 1000)
    pinch(s);
  
 // Serial.println(angle);
  Serial.println(val);
  delay(6);

}

void grab(ServoPWMF sm)
{
  SMwriteAll(sm, 45,180,180,180,180);
}


void pinch(ServoPWMF sm)
{
  SMwriteAll(sm, 115,180,90,90,90);
}


//COUNT AND POINT
void point(ServoPWMF sm)
{
  SMwriteAll(sm, 45,90,180,180,180);
}

void count2(ServoPWMF sm)
{
  SMwriteAll(sm,45,90,90,180,180);
}
void count3(ServoPWMF sm)
{
  SMwriteAll(sm,45,90,90,90,180);
}
void count4(ServoPWMF sm)
{
  SMwriteAll(sm,45,90,90,90,90);
}

void SMwriteAll (ServoPWMF sm,int t, int i, int m, int p, int r)
{
 sm.writeT(t);
 sm.writeF1(i);
 sm.writeF2(m);
 sm.writeF3(p);
 sm.writeF4(r);  
}  
